#include "STLModelAttributesCalculator.h"
#include "ModelFormation.h"

QVector3D STLModelAttributesCalculator::calModelCentroid(const QVector<Mesh>& stl_model_meshes)
{
    float totalArea = 0.0f; // 总面积
    QVector3D weightedCentroid(0.0f, 0.0f, 0.0f); // 加权质心

    // 遍历所有三角形面片
    for (const Mesh& mesh : stl_model_meshes)
    {
        // 获取三角形的三个顶点
        QVector3D v1 = mesh.v1;
        QVector3D v2 = mesh.v2;
        QVector3D v3 = mesh.v3;

        // 计算三角形的质心（三个顶点的平均值）
        QVector3D centroid = (v1 + v2 + v3) / 3.0f;

        // 计算三角形的面积（通过叉积计算）
        QVector3D edge1 = v2 - v1;
        QVector3D edge2 = v3 - v1;
        QVector3D crossProduct = QVector3D::crossProduct(edge1, edge2);
        float area = 0.5f * crossProduct.length(); // 三角形面积

        // 累加加权质心
        weightedCentroid += area * centroid;
        totalArea += area;
    }

    // 计算最终质心
    if (totalArea > 0.0f)
        return weightedCentroid / totalArea;
    else
        return QVector3D(0.0f, 0.0f, 0.0f); // 如果面积为 0，返回默认值
}

// 手动实现 3x3 矩阵的特征值分解（雅可比迭代法）
void jacobiEigenDecomposition(float matrix[3][3], float eigenvalues[3], float eigenvectors[3][3])
{
    const int maxIterations = 100;
    const float epsilon = 1e-6f;

    // 初始化特征向量矩阵为单位矩阵
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            eigenvectors[i][j] = (i == j) ? 1.0f : 0.0f;

    // 雅可比迭代
    for (int iter = 0; iter < maxIterations; ++iter)
    {
        // 找到最大的非对角元素
        int p = 0, q = 1;
        float maxVal = fabs(matrix[0][1]);
        for (int i = 0; i < 3; ++i)
            for (int j = i + 1; j < 3; ++j)
                if (fabs(matrix[i][j]) > maxVal)
                {
                    maxVal = fabs(matrix[i][j]);
                    p = i;
                    q = j;
                }

        // 如果最大非对角元素已经很小，停止迭代
        if (maxVal < epsilon) break;

        // 计算旋转角度
        float theta = 0.5f * atan2(2 * matrix[p][q], matrix[q][q] - matrix[p][p]);
        float c = cos(theta);
        float s = sin(theta);

        // 更新矩阵
        float newMatrix[3][3];
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                if (i == p && j == p)
                    newMatrix[i][j] = c * c * matrix[p][p] - 2 * s * c * matrix[p][q] + s * s * matrix[q][q];
                else if (i == q && j == q)
                    newMatrix[i][j] = s * s * matrix[p][p] + 2 * s * c * matrix[p][q] + c * c * matrix[q][q];
                else if ((i == p && j == q) || (i == q && j == p))
                    newMatrix[i][j] = (c * c - s * s) * matrix[p][q] + s * c * (matrix[p][p] - matrix[q][q]);
                else
                    newMatrix[i][j] = matrix[i][j];

        // 更新特征向量矩阵
        float newEigenvectors[3][3];
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                newEigenvectors[i][j] = eigenvectors[i][j];

        for (int i = 0; i < 3; ++i)
        {
            newEigenvectors[i][p] = c * eigenvectors[i][p] - s * eigenvectors[i][q];
            newEigenvectors[i][q] = s * eigenvectors[i][p] + c * eigenvectors[i][q];
        }

        // 更新矩阵和特征向量
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
            {
                matrix[i][j] = newMatrix[i][j];
                eigenvectors[i][j] = newEigenvectors[i][j];
            }
    }

    // 提取特征值
    for (int i = 0; i < 3; ++i)
        eigenvalues[i] = matrix[i][i];
}

QVector3D STLModelAttributesCalculator::calModelInitPhaseNorm(const QVector<Mesh>& stl_model_meshes) {
    // 提取所有顶点
    QVector<QVector3D> vertices;
    for (const Mesh& mesh : stl_model_meshes)
    {
        vertices.append(mesh.v1);
        vertices.append(mesh.v2);
        vertices.append(mesh.v3);
    }

    // 计算顶点集的均值（中心点）
    QVector3D mean(0.0f, 0.0f, 0.0f);
    for (const QVector3D& vertex : vertices)
        mean += vertex;
    mean /= vertices.size();

    // 构建协方差矩阵
    float covariance[3][3] = {0};
    for (const QVector3D& vertex : vertices)
    {
        QVector3D diff = vertex - mean;
        covariance[0][0] += diff.x() * diff.x();
        covariance[0][1] += diff.x() * diff.y();
        covariance[0][2] += diff.x() * diff.z();
        covariance[1][0] += diff.y() * diff.x();
        covariance[1][1] += diff.y() * diff.y();
        covariance[1][2] += diff.y() * diff.z();
        covariance[2][0] += diff.z() * diff.x();
        covariance[2][1] += diff.z() * diff.y();
        covariance[2][2] += diff.z() * diff.z();
    }
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            covariance[i][j] /= vertices.size();

    // 手动计算特征值和特征向量
    float eigenvalues[3];
    float eigenvectors[3][3];
    jacobiEigenDecomposition(covariance, eigenvalues, eigenvectors);

    // 找到最大特征值对应的特征向量
    int maxIndex = 0;
    float maxEigenvalue = eigenvalues[0];
    for (int i = 1; i < 3; ++i)
        if (eigenvalues[i] > maxEigenvalue)
        {
            maxEigenvalue = eigenvalues[i];
            maxIndex = i;
        }

    // 返回主方向
    return QVector3D(eigenvectors[0][maxIndex], eigenvectors[1][maxIndex], eigenvectors[2][maxIndex]);
}
